/******************************************************************************
 *	Author: 	Williams, Jacob (williamsj@msoe.edu)
 *	Class:		CE-3910
 *	Instructor:	Dr. Barnekow
 *****************************************************************************/
#include <stdio.h> // allows use of fgets(), printf(), etc.
#include <stdlib.h>
#include <string.h> // allows use of strcmp() and other string functions
#include <avr/io.h>
#include <stdbool.h>
#include <ctype.h>
#include "uart.h"
#include "servo.h"
#include "twi.h"
#include "camera.h"
// Control Characters
#define ENTER_CHARACTER '\r'
#define BACKSPACE_CHARACTER 0x7f
#define DELETE_CHARACTER 0x7f
#define NULL_CHARACTER '\0'
#define BELL_CHARACTER 0x07
//Argument Descriptions
#define HELP_COMMAND "HELP"
#define WRIO_COMMAND "WRIO"
#define WRIO_ARG_COUNT 3
#define RDIO_COMMAND "RDIO"
#define RDIO_ARG_COUNT 2
#define TILT_COMMAND "TILT"
#define TILT_ARG_COUNT 2
#define PAN_COMMAND "PAN"
#define PAN_ARG_COUNT 2
#define SET_CAMERA_COMMAND "SETCAM"
#define SET_CAMERA_ARG_COUNT 3
#define READ_CAMERA_COMMAND "READCAM"
#define READ_CAMERA_ARG_COUNT 2
#define CALIBRATE_COMMAND "CALIBRATE"
#define CALIBRATE_ARG_COUNT 2
#define CALIBRATE_ARG_WHITE "WHITE"
#define CALIBRATE_ARG_BLACK "BLACK"
#define TRACK_COMMAND "TRACK"
#define TRACK_ARG_COUNT 1
// Defines how large the command buffer is
#define BUFFER_SIZE 20
// Limits that cause the camera to move based on the location of the
// white blob on the black background
#define X_CENTER 12
#define X_THRESHHOLD 2
#define Y_CENTER 72
#define Y_THRESHHOLD 5
// Needed to make printf print out the UART
FILE uart_str = FDEV_SETUP_STREAM(uartPutc, uartGetc, _FDEV_SETUP_RW);

// Function prototypes
void resetCommand(void);
void executeCommand(void);
uint8_t readIO(uint8_t regAddress);
void writeIO(uint8_t regaddress, uint8_t data);

// used to hold commands as the user types them
char commandBuffer[BUFFER_SIZE];
char bufferSpot;

/**
 * Provides the main work for camera program. Accepts different commands.
 * Data is received through the UART and echoed back. When entered is
 * pressed the command s executed with the provided parameters.
 */
int main(void) {

    // Initialize the USART communication subsystem
    uartInit();

    // set STDOUT and STDIN to the USART so we can use getf and printf
    stdout = stdin = &uart_str;

    initServo();

    cameraInit();

    // Clear the command buffer
    resetCommand();

    // used to store the last sent character
    char lastSent;

    // Say hello to the user to show that the system has just underwent a startup
    printf("\n\rHello!\n\r");

    // Main program body
    while (1) {

        // get the next character
        lastSent = uartGetc();

        // Check if it is the ENTER key
        // if it is execute the command in the buffer
        if (lastSent == ENTER_CHARACTER) {

            executeCommand();

            // Check if it is the BACKSPACE key,
            // if it is delete a character from the command buffer
        } else if (lastSent == BACKSPACE_CHARACTER) {

            // Don't go past the start of the command buffer
            if (bufferSpot != 0) {

                // decrement the spot in the buffer
                bufferSpot = bufferSpot - 1;

                // clear the spot in the buffer
                commandBuffer[bufferSpot] = '\0';

                // delete the character from the terminal
                printf("%c", BACKSPACE_CHARACTER);

            } else {

                // Inform the user they cannot go back
                // any further by making a noise
                printf("%c", BELL_CHARACTER);

            }

            // Add the character to the command buffer
        } else {

            // Don't exceed the buffer length
            if (bufferSpot < BUFFER_SIZE) {

                // if the last sent character is an alphabetic character convert
                // it to upper case before adding it to the buffer
                if (isalpha(lastSent)) {

                    commandBuffer[bufferSpot] = toupper(lastSent);

                    // if it is not just add it to the commandBuffer
                } else {

                    commandBuffer[bufferSpot] = lastSent;

                }

                // echo back the last sent character
                printf("%c", commandBuffer[bufferSpot]);

                // increment where we are in the buffer
                bufferSpot++;

            } else {

                // notify the user that the buffer is full
                // by making the bell noise at them
                printf("%c", BELL_CHARACTER);

            }

        }

    }

}

/**
 * This function clears the command buffer. This includes setting the memory
 * to all null and clearing the variable that stores the current location
 * in the buffer
 */
void resetCommand(void) {

    // set each character to null
    memset(commandBuffer, NULL_CHARACTER, BUFFER_SIZE);

    // Set the spot in the command buffer to the head of the command buffer
    bufferSpot = 0;

}

/**
 * This function executes the command currently stored in the command buffer
 */
void executeCommand(void) {

    // used to store the command when read using sscanf
    char command[BUFFER_SIZE];

    // stores the address from the command
    uint8_t param1;

    // stores the data from the command
    uint8_t param2;

    // stores the number of arguments read using sscanf
    uint8_t argCount;

    // Check for each of the different commands
    // Check for the Read IO Command (RDIO ADDRESS)
    if (strncmp(commandBuffer, RDIO_COMMAND, strlen(RDIO_COMMAND))
            == 0) {

        // get the arguments
        argCount = sscanf(commandBuffer, "%s %X", &command, &param1);

        // if the right number of arguments were provided execute the command,
        // otherwise print a usgae statement
        if (argCount == RDIO_ARG_COUNT) {

            // read from the IO port and display it to the user
            printf("\n\rIOdata: %X\n\r", readIO(param1));

        } else {

            printf("\n\rUsage: RDIO ADDRESS\n\r");

        }

        // check for the Write IO Command (WRIO ADDRESS VALUE)
    } else if (strncmp(commandBuffer, WRIO_COMMAND, strlen(WRIO_COMMAND))
            == 0) {

        // get the arguments
        argCount = sscanf(commandBuffer, "%s %X %X", command, &param1, &param2);

        // if the right number of arguments were provided execute the command.
        // Otherwise print a usage statement
        if (argCount == WRIO_ARG_COUNT) {

            // Write to the IO port
            writeIO(param1, param2);

            printf("\n\rIOport: %X IOdata: %X\n\r", param1, param2);

        } else {

            printf("\n\rUsage: WRIO ADDRESS DATA\n\r");

        }

        // Check for the Tilt command (SETTILT DEGREES)
    } else if (strncmp(commandBuffer, TILT_COMMAND, strlen(TILT_COMMAND))
            == 0) {

        // get the command and arguments
        argCount = sscanf(commandBuffer, "%s %d", command, &param1);

        // if the correct number of arguments were passed execute the command
        // by setting the current camera tilt to DEGREE
        if (argCount == TILT_ARG_COUNT) {

            setTilt(param1);

            printf("\n\rTilted to %d\n\r", getTilt());

        } else {

            printf("\n\rUsage: TILT ANGLE\n\r");

        }

        // Check for the Pan Command (SETPAN DEGREES
    } else if (strncmp(commandBuffer, PAN_COMMAND, strlen(PAN_COMMAND)) == 0) {

        // get the command and arguments from the buffer
        argCount = sscanf(commandBuffer, "%s %d", command, &param1);

        // if the correct number of parameters were passed execute the command
        if (argCount == PAN_ARG_COUNT) {

            setPan(param1);

            printf("\n\rPanned to %d\n\r", getPan());

        } else {

            printf("\n\rUsage: PAN ANGLE\n\r");

        }

        // Check for the Set Camera Register Command (SETCAM ADDRESS VALUE)
    } else if (strncmp(commandBuffer, SET_CAMERA_COMMAND,
            strlen(SET_CAMERA_COMMAND)) == 0) {

        // get the command and arguments from the buffer
        argCount = sscanf(commandBuffer, "%s %X %X", command, &param1, &param2);

        // if the correct number of parameters were passed execute the command
        if (argCount == SET_CAMERA_ARG_COUNT) {

            TWIwrite(param1, param2);

            printf("\n\rCamera register 0x%X set to 0x%X\n\r", param1, param2);

        } else {

            printf("\n\rSETCAM ADDRESS DATA\n\r");

        }

        // Check for the Read Camera Register Command (READCAM ADDRESS)
    } else if (strncmp(commandBuffer, READ_CAMERA_COMMAND,
            strlen(READ_CAMERA_COMMAND)) == 0) {

        // get the command and arguments from the buffer
        argCount = sscanf(commandBuffer, "%s %X %X", command, &param1);

        // if the correct number of parameters were passed execute the command
        if (argCount == READ_CAMERA_ARG_COUNT) {

            param2 = TWIread(param1);

            printf("\n\rCamera address 0x%X is 0x%X\n\r", param1, param2);

        } else {

            printf("\n\rREADCAM ADDRESS\n\r");

        }

        // Check for the calibrate command
    } else if (strncmp(commandBuffer, CALIBRATE_COMMAND,
            strlen(CALIBRATE_COMMAND)) == 0) {

        // stores the color passed as a argument
        char color[10];

        // break up the command and argument
        argCount = sscanf(commandBuffer, "%s %s", command, &color);

        // check that the correct number of arguments were passed
        if (argCount == CALIBRATE_ARG_COUNT) {

            // If white was the parameter calibrate white
            if (strncmp(color, CALIBRATE_ARG_WHITE, strlen(CALIBRATE_ARG_WHITE))
                    == 0) {

                calibrate(true);

                printf(
                        "\n\rCamera White color set to: 0x%X threshhold: 0x%X\n\r",
                        getWhite(), getThresh());

                // else if black was the parameter calibrate black
            } else if (strncmp(color, CALIBRATE_ARG_BLACK,
                    strlen(CALIBRATE_ARG_BLACK)) == 0) {

                calibrate(false);

                printf(
                        "\n\rCamera Black color set to: 0x%X threshhold: 0x%X\n\r",
                        getBlack(), getThresh());

            } else {

                printf("\n\rCALIBRATE [BLACK/WHITE]\n\r");

            }

        } else {

            printf("\n\rCALIBRATE [BLACK/WHITE]\n\r");

        }

        // Check for the Track Command
    } else if (strncmp(commandBuffer, TRACK_COMMAND, strlen(TRACK_COMMAND))
            == 0) {

        printf("\n\rNw Tracking\n\r");

        // Continue tracking from here on out
        while (1) {

            // process the next available frame
            processImage();

            // get the center points of the white blob
            uint8_t imageX = getObjectXCenter();
            uint8_t imageY = getObjectYCenter();

            // pan based on the location of the white block
            if (imageX > (X_CENTER + X_THRESHHOLD)) {

                setPan(getPan() - 1);

            } else if (getObjectXCenter() < X_CENTER - X_THRESHHOLD) {

                setPan(getPan() + 1);

            }

            // tilt based on the location of the white block
            if (getObjectYCenter() > X_CENTER + Y_THRESHHOLD) {

                setTilt(getTilt() + 1);

            } else if (getObjectYCenter() < X_CENTER - Y_THRESHHOLD) {

                setTilt(getTilt() - 1);

            }

        }

        // If no command was recognized inform the user
    } else {

        printf("\n\rUnrecognized Command!\n\r");

    }

    // clear the command buffer
    resetCommand();

}

/**
 * Reads the value of the specified register
 * @param regAddress The address of the register to be read
 * @return The value of the specified register
 */
uint8_t readIO(uint8_t regAddress) {

    // create a pointer to the specified address
    volatile unsigned char* ioPort = (unsigned char*) regAddress;

    // return the value at the IO port
    return *ioPort;

}

/**
 * Writes the specified value to the specified register
 * @param regAddress The register to set the value of
 * @param data The value to set the specified register to
 */
void writeIO(uint8_t regAddress, uint8_t data) {

    // create a pointer to the specified address
    volatile unsigned char* ioPort = (unsigned char*) regAddress;

    // set the value at that address to specified data
    *ioPort = data;

}
